/*
 * tracking_control.h
 *
 *  Created on: 2021年3月24日
 *      Author: Jiarun
 */

#ifndef CODE_TRACKING_CONTROL_H_
#define CODE_TRACKING_CONTROL_H_


#include "img_processing.h"
#include "module_init.h"
#include "zf_gtm_pwm.h"
#include "zf_gpt12.h"
#include "zf_gpio.h"


typedef struct motor
{
  int16 speed_real;
  int16 speed_set;
  float P;
  float I;
  float D;
  int16 lastError;
  int16 preError;
  int16 out;
  int16 PWM;
}Motor;

typedef struct Param_Memory
{
    /* SetParameter 中的变量  */
    uint8 speed_max[5];//直道速度
    uint8 speed_normal[5];//正常速度
    uint8 speed_round[5];//环岛速度

    uint8 kp[5];//舵机P
    uint8 kd[5];//舵机D
    uint8 speed_level;
    uint8 advance;

    /* Element_SW 中的变量 */
    uint8 road_sw;    //禁用摄像头
    uint8 run_dir;          //发车方向
    uint8 Y_dir;            //首次入三岔的行进方向
    uint8 AUTO_EXP;
    uint8 GAIN;
    uint8 Straight_s;
    uint8 ramp_s;
}Param;

/* 电机不转, 用于测试编码器 */
//#define  PWM_DISABLE


#define BIAS        46u

#define Servo_Median    4600    //4550,左减右加
#define Angle_min       3800
#define Angle_max       5400
#define Servo_PIN       ATOM0_CH1_P33_9

#define Speed_Max       6000
#define MOTOR_PIN       ATOM1_CH0_P21_2
#define MOTOR_INVER     ATOM1_CH1_P21_3

#define      buzzer_ON     gpio_set(P33_5,1)
#define      buzzer_OFF    gpio_set(P33_5,0)


extern Param global_param;
extern Motor pid;
extern uint8 run_start, stopcar, finish, finish_sw, road_flag, road_sw, road_dir, ramp_flag, debug_flag;
extern uint16 zebra, ramp_distance;
extern int16 distance;
extern uint32 ignore_time, road_time;

void motor_ctrl(void);
void servo_ctrl(void);
int16 constrain_int16(int16 amt, int16 low, int16 high);
void motor_PID(void);
void roundabout(void);


#endif /* CODE_TRACKING_CONTROL_H_ */
